#pragma systemFile
/********************************************************\
|*  NXC For ROBOTC Virtual World                        *|
|*  This library contains list of macros and functions  *|
|*  to emulate NXC API as so you can write programs on  *|
|*  NXC language and run it in RobotC Virtual World     *|
|*                                                      *|
|*  version: 0.02                                       *|
|*                                                      *|
|*  Author: Alexander Kolotov                           *|
|*   email: alexandr.kolotov@gmail.com                  *|
|*                                                      *|
|*  NXC subroutines definition with "sub" keyword is    *|
|*  supported.                                          *|
|*  List of covered NXC constants and functions         *|
|*  Constants: OUT_A, OUT_B, OUT_C,                     *|
|*             OUT_AB, OUT_BC, OUT_AC, OUT_ABC,         *|
|*             SENSOR_1, SENSOR_2, SENSOR_3, SENSOR_4   *|
|*  Functions: Sensor, SensorUS, OnFwd, OnFwdSync,      *|
|*             OnRev, Off, Wait, RotateMotor,           *|
|*             RotateMotorEx, SetSensorLight,           *|
|*             SetSensorLowspeed, SetSensorTouch        *|
|*             SensorValue, OnRevSync, ResetTachoCount  *|
|*             MotorTachoCount                          *|
\********************************************************/

#define sub void

#define OUT_A   1
#define OUT_B   2
#define OUT_C   4
#define OUT_AB  OUT_A+OUT_B
#define OUT_BC  OUT_B+OUT_C
#define OUT_AC  OUT_A+OUT_C
#define OUT_ABC OUT_A+OUT_B+OUT_C

#define Sensor    SensorValue
#define SensorUS  SensorValue
#define SENSOR_1  SensorValue(S1)
#define SENSOR_2  SensorValue(S2)
#define SENSOR_3  SensorValue(S3)
#define SENSOR_4  SensorValue(S4)

#define PowerLeftM(x, y) ((y + 50) * 0.02 * (x))
#define PowerRightM(x, y) ((50 - y) * 0.02 * (x))

void OnFwd(int Motors, int Power) {
  if (Power >= 0) {
	  if (Motors & OUT_A) { motor[motorA]=Power; }
	  if (Motors & OUT_B) { motor[motorB]=Power; }
	  if (Motors & OUT_C) { motor[motorC]=Power; }
  }
}

void OnFwdSync(int Motors, int Power, int Turn) {
  if (Power >= 0) {
	  if (Turn < 0) {
	    //-100 the left motor goes in reverse direction
	    //-50 the left motor is not moving
      if (Motors == OUT_A+OUT_B) { motor[motorA]=Power; motor[motorB]=PowerLeftM(Power, Turn); }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=Power; motor[motorC]=PowerLeftM(Power, Turn); }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=Power; motor[motorC]=PowerLeftM(Power, Turn); }
	  } else if (Turn > 0) {
	    //100 the right motor goes in reverse direction
	    //50 the right motor is not moving
      if (Motors == OUT_A+OUT_B) { motor[motorA]=PowerRightM(Power, Turn); motor[motorB]=Power; }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=PowerRightM(Power, Turn); motor[motorC]=Power; }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=PowerRightM(Power, Turn); motor[motorC]=Power; }
	  } else {
      if (Motors == OUT_A+OUT_B) { motor[motorA]=Power; motor[motorB]=Power; }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=Power; motor[motorC]=Power; }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=Power; motor[motorC]=Power; }
	  }
  }
}

void OnRev(int Motors, int Power) {
  if (Power >= 0) {
	  if (Motors & OUT_A) { motor[motorA]=-Power; }
	  if (Motors & OUT_B) { motor[motorB]=-Power; }
	  if (Motors & OUT_C) { motor[motorC]=-Power; }
  }
}

void OnRevSync(int Motors, int Power, int Turn) {
  if (Power >= 0) {
	  if (Turn < 0) {
	    //-100 the left motor goes in reverse direction
	    //-50 the left motor is not moving
      if (Motors == OUT_A+OUT_B) { motor[motorA]=-Power; motor[motorB]=PowerLeftM(-Power, Turn); }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=-Power; motor[motorC]=PowerLeftM(-Power, Turn); }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=-Power; motor[motorC]=PowerLeftM(-Power, Turn); }
	  } else if (Turn > 0) {
	    //100 the right motor goes in reverse direction
	    //50 the right motor is not moving
      if (Motors == OUT_A+OUT_B) { motor[motorA]=PowerRightM(-Power, Turn); motor[motorB]=-Power; }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=PowerRightM(-Power, Turn); motor[motorC]=-Power; }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=PowerRightM(-Power, Turn); motor[motorC]=-Power; }
	  } else {
      if (Motors == OUT_A+OUT_B) { motor[motorA]=-Power; motor[motorB]=-Power; }
      if (Motors == OUT_B+OUT_C) { motor[motorB]=-Power; motor[motorC]=-Power; }
      if (Motors == OUT_A+OUT_C) { motor[motorA]=-Power; motor[motorC]=-Power; }
	  }
  }
}

void Off(int Motors) {
  if (Motors & OUT_A) { motor[motorA]=0; }
  if (Motors & OUT_B) { motor[motorB]=0; }
  if (Motors & OUT_C) { motor[motorC]=0; }
}

void RotateMotor(int Motors, int Power, int Angle) {
  if ((Power == 0) || (Angle == 0)) return;

  if (Motors & OUT_A) { nMotorEncoder[motorA]=0; }
  if (Motors & OUT_B) { nMotorEncoder[motorB]=0; }
  if (Motors & OUT_C) { nMotorEncoder[motorC]=0; }

  int Direction = sgn(Angle);

  if (Motors & OUT_A) { motor[motorA]=Power*Direction; }
  if (Motors & OUT_B) { motor[motorB]=Power*Direction; }
  if (Motors & OUT_C) { motor[motorC]=Power*Direction; }

  bool StopMoving = true;

  do {
    StopMoving = true;
	  if (Motors & OUT_A) {
	    if (abs(nMotorEncoder[motorA]) < abs(Angle)) { StopMoving = false; }
	  }
	  if (Motors & OUT_B) {
	    if (abs(nMotorEncoder[motorB]) < abs(Angle)) { StopMoving = false; }
	  }
	  if (Motors & OUT_C) {
	    if (abs(nMotorEncoder[motorC]) < abs(Angle)) { StopMoving = false; }
	  }
  } while (!StopMoving);

  Off(Motors);
}

void RotateMotorEx(int Motors, int Pwr, int Angle, int Turn, bool Sync = true, bool Stop = true) {
  if ((Pwr == 0) || (Angle == 0)) return;
  if ((Motors == OUT_A) || (Motors == OUT_B) || (Motors == OUT_C)) return;
  if (Motors == OUT_ABC) return;

  if (Motors & OUT_A) { nMotorEncoder[motorA]=0; }
  if (Motors & OUT_B) { nMotorEncoder[motorB]=0; }
  if (Motors & OUT_C) { nMotorEncoder[motorC]=0; }

  int Direction = sgn(Angle);

  int Power = Pwr*Direction;

  if (Turn < 0) {
    //-100 the left motor goes in reverse direction
    //-50 the left motor is not moving
    if (Motors == OUT_A+OUT_B) { motor[motorA]=Power; motor[motorB]=PowerLeftM(Power, Turn); }
    if (Motors == OUT_B+OUT_C) { motor[motorB]=Power; motor[motorC]=PowerLeftM(Power, Turn); }
    if (Motors == OUT_A+OUT_C) { motor[motorA]=Power; motor[motorC]=PowerLeftM(Power, Turn); }
    } else if (Turn > 0) {
    //100 the right motor goes in reverse direction
    //50 the right motor is not moving
    if (Motors == OUT_A+OUT_B) { motor[motorA]=PowerRightM(Power, Turn); motor[motorB]=Power; }
    if (Motors == OUT_B+OUT_C) { motor[motorB]=PowerRightM(Power, Turn); motor[motorC]=Power; }
    if (Motors == OUT_A+OUT_C) { motor[motorA]=PowerRightM(Power, Turn); motor[motorC]=Power; }
    } else {
    if (Motors == OUT_A+OUT_B) { motor[motorA]=Power; motor[motorB]=Power; }
    if (Motors == OUT_B+OUT_C) { motor[motorB]=Power; motor[motorC]=Power; }
    if (Motors == OUT_A+OUT_C) { motor[motorA]=Power; motor[motorC]=Power; }
  }

  bool StopMoving = true;

  do {
    StopMoving = true;
    if (Turn < 0) {
      if (Motors == OUT_A+OUT_B) { if (abs(nMotorEncoder[motorA]) < abs(Angle)) { StopMoving = false; } }
      if (Motors == OUT_B+OUT_C) { if (abs(nMotorEncoder[motorB]) < abs(Angle)) { StopMoving = false; } }
      if (Motors == OUT_A+OUT_C) { if (abs(nMotorEncoder[motorA]) < abs(Angle)) { StopMoving = false; } }
    } else if (Turn > 0) {
      if (Motors == OUT_A+OUT_B) { if (abs(nMotorEncoder[motorB]) < abs(Angle)) { StopMoving = false; } }
      if (Motors == OUT_B+OUT_C) { if (abs(nMotorEncoder[motorC]) < abs(Angle)) { StopMoving = false; } }
      if (Motors == OUT_A+OUT_C) { if (abs(nMotorEncoder[motorC]) < abs(Angle)) { StopMoving = false; } }
    } else {
		  if (Motors & OUT_A) {
		    if (abs(nMotorEncoder[motorA]) < abs(Angle)) { StopMoving = false; }
		  }
		  if (Motors & OUT_B) {
		    if (abs(nMotorEncoder[motorB]) < abs(Angle)) { StopMoving = false; }
		  }
		  if (Motors & OUT_C) {
		    if (abs(nMotorEncoder[motorC]) < abs(Angle)) { StopMoving = false; }
		  }
	  }
  } while (!StopMoving);

  if (Stop) Off(Motors);
}

void ResetTachoCount(int Motors) {
  if (Motors & OUT_A) { nMotorEncoder[motorA]=0; }
  if (Motors & OUT_B) { nMotorEncoder[motorB]=0; }
  if (Motors & OUT_C) { nMotorEncoder[motorC]=0; }
}

long MotorTachoCount(int output) {
  if (output == OUT_A) { return nMotorEncoder[motorA]; }
  if (output == OUT_B) { return nMotorEncoder[motorB]; }
  if (output == OUT_C) { return nMotorEncoder[motorC]; }

  return -1;
}

void Wait(int Milliseconds) {
  wait1Msec(Milliseconds);
}

void SetSensorLight(tSensors input) {
  //No action here
}

void SetSensorLowspeed(tSensors input) {
  //No action here
}

void SetSensorTouch(tSensors input) {
  //No action here
}
